package cn.chenxing.module.file.preprocess;

import cn.chenxing.domain.dto.TrackData;
import cn.chenxing.domain.dto.TrackLine;
import cn.chenxing.domain.dto.TrackPoint;
import cn.chenxing.domain.threadlocal.ThreadData;

import java.math.BigDecimal;
import java.util.List;

/**
 * @Description 卡尔曼滤波
 * @Author maogen.ymg
 * @Date 2020/4/6 23:22
 */
public class KalManFilter {
    /**
     * 时间戳
     **/
    private long timeStamp;
    /**
     * 经度（degree）
     */
    private double longitude;
    /**
     * 纬度（degree）
     */
    private double latitude;
    /**
     * P matrix. Initial estimate of error
     */
    private float variance = -1;

    /**
     * 最小精度 -- 数值越小->卡尔曼滤波算法对最终数据的影响越小
     */
    private static final float MIN_ACCURACY = 2f;

    /**
     * 设置初始状态
     * @param latitude 纬度
     * @param longitude 经度
     * @param timeStamp 时间戳
     * @param accuracy 精度
     */
    private void setState(double latitude, double longitude, long timeStamp, float accuracy) {
        this.latitude = latitude;
        this.longitude = longitude;
        this.timeStamp = timeStamp;
        this.variance = accuracy * accuracy;
    }

    /**
     * 卡尔曼滤波
     */
    public void kalManFilter() {
        // 所有用户数据
        TrackData trackData = ThreadData.get().getTrackData();
        // 用户数据
        List<TrackLine> trackLineList = trackData.getTrackLineList();

        for(int i = 0; i < trackLineList.size(); i++) {
            // 单个用户数据详情
            TrackLine trackLine = trackLineList.get(i);

            List<TrackPoint> compressCoordinate = trackLine.getTrackPointList();
            for(int j = 0; j < compressCoordinate.size(); j++) {
                // 轨迹点
                TrackPoint trackPoint = compressCoordinate.get(j);
                TrackPoint tempTrackPoint = kalManFilter(trackPoint.getSpeed(), trackPoint.getLongitude(),
                        trackPoint.getLatitude(), trackPoint.getTimeStamp(), MIN_ACCURACY);
                trackPoint.setLongitude(decimalValue(tempTrackPoint.getLongitude(),5));
                trackPoint.setLatitude(decimalValue(tempTrackPoint.getLatitude(),5));
                compressCoordinate.set(j, trackPoint);
            }

            trackLine.setTrackPointList(compressCoordinate);
            trackLineList.set(i, trackLine);
        }

        trackData.setTrackLineList(trackLineList);
        ThreadData.get().setTrackData(trackData);
    }

    /**
     * 卡尔曼滤波处理
     * @param newSpeed 速度
     * @param newLongitude 经度
     * @param newLatitude 纬度
     * @param newTimeStamp 时间戳
     * @param newAccuracy 精度 -- 1米标准差误差
     */
    private TrackPoint kalManFilter(double newSpeed, double newLongitude,
                                          double newLatitude, long newTimeStamp, float newAccuracy) {

        if (variance < 0) {
            // if variance < 0, 用当前值初始化
            setState(newLatitude, newLongitude, newTimeStamp, newAccuracy);
        } else {
            // 时间差
            long duration = newTimeStamp - this.timeStamp;
            // 随着时间的推移，当前位置的精度增加
            if (duration > 0) {
                variance += duration * newSpeed * newSpeed / 1000;
                timeStamp = newTimeStamp;
            }

            // KalMan gain matrix 'k' = Covariance * Inverse(Covariance + MeasurementVariance)
            // 卡尔曼增益矩阵k = 协方差*逆（协方差+测量变量）
            float k = variance / (variance + newAccuracy * newAccuracy);
            // k is dimensionless, variance对经纬度有不同的单位并不重要
            latitude += k * (newLatitude - latitude);
            longitude += k * (newLongitude - longitude);
            // new Covariance matrix is (IdentityMatrix - k) * Covariance
            // 新协方差矩阵是（同一矩阵 - k) * 协方差
            variance = (1 - k) * variance;
        }

        TrackPoint trackPoint = new TrackPoint();
        trackPoint.setLongitude(longitude);
        trackPoint.setLatitude(latitude);
        return trackPoint;
    }

    /**
     * 小数点保留n位
     * @param d 数值
     * @param newScale 保留位数
     * @return Double
     */
    private static double decimalValue(double d, int newScale) {
        BigDecimal bd = BigDecimal.valueOf(d).setScale(newScale, BigDecimal.ROUND_HALF_UP);
        return bd.doubleValue();
    }

}
